﻿#pragma once
#include "pyramid_exclusive.h"

#include <pcl/console/print.h>
#include <pcl/common/common.h>
#include <pcl/filters/extract_indices.h>

#include <vector>
#include <algorithm>

#include <Eigen/Core>

#include <boost/make_shared.hpp>

using namespace std;

namespace oil
{
template <typename PointT>
PyramidExclusive<PointT>::PyramidExclusive(float min_window, float max_window, float step)
	: min_window_(min_window), max_window_(max_window), step_(step)
{
	name_ = "PyramidExclusive";
}

template <typename PointT>
PyramidExclusive<PointT>::~PyramidExclusive()
{
}

template <typename PointT>
void PyramidExclusive<PointT>::compute(std::vector<PointCloudPtr>& output)
{

}


template <typename PointT>
void PyramidExclusive<PointT>::compute(vector<vector<int>>& indices)
{
	std::cout << "compute pyramid index" << std::endl;
	if (!initCompute())
	{
		PCL_ERROR("[pcl::%s::compute] initCompute failed!\n", getClassName().c_str());
		return;
	}
	//将金字塔level从大到小排序
	std::sort(levels_.begin(), levels_.end(), std::greater<float>());
	indices.resize(levels_.size() + 1); //最后还要存一个没有organized 点的索引

	std::vector<int> unorganized_indices(indices_->size());
	for (int i = 0; i < unorganized_indices.size(); ++i)
		unorganized_indices[i] = i;

	for (int i = 0; i < levels_.size(); ++i)
	{
		float window_size = levels_[i];
		int width = std::floor((max_p_.x() - min_p_.x()) / window_size) + 1;
		int height = std::floor((max_p_.y() - min_p_.y()) / window_size) + 1;

		std::vector<int> next;
		organizeInput(unorganized_indices, window_size, next);

		// 把next 从 unorganized_indices去除掉
		sort(begin(unorganized_indices), end(unorganized_indices));
		sort(begin(next), end(next));

		const auto& result_end = set_difference(begin(unorganized_indices),
			end(unorganized_indices), begin(next), end(next),
			begin(unorganized_indices));
		unorganized_indices.erase(result_end, end(unorganized_indices));

		//把output存入结果
		transform(begin(next), end(next), begin(next), [this](int& v){
			return (*indices_)[v];
		});
		indices[i] = next;
	}
	transform(begin(unorganized_indices), end(unorganized_indices),
		begin(unorganized_indices), [this](int& v){
		return (*indices_)[v];
	});
	indices[levels_.size()] = unorganized_indices;
	// 	int total = 0;
	// 	for_each(begin(indices), end(indices),[&total](const vector<int>& v){
	// 		total += v.size();
	// 	});
	// 	cout << "total number of points in pyramid is :" << total << endl;
}

template <typename PointT>
bool PyramidExclusive<PointT>::initCompute()
{
	if (min_window_ < 0 || max_window_ < 0 || max_window_ < min_window_)
	{
		PCL_ERROR("[pcl::fileters::%s::initCompute] window size error!", getClassName().c_str());
		return (false);
	}
	//计算包围和
	pcl::getMinMax3D(*input_, min_p_, max_p_);

	//计算levels
	levels_.clear();
	float window_size = min_window_;
	while (window_size < max_window_)
	{
		levels_.push_back(window_size);
		window_size *= step_;
	}
	levels_.push_back(max_window_);

	return (true);
}

template <typename PointT>
void PyramidExclusive<PointT>::organizeInput(const PointCloudPtr& input, float window_size, PointCloudPtr& output)
{

}

template <typename PointT>
void PyramidExclusive<PointT>::organizeInput(const std::vector<int>& input, float window_size, std::vector<int>& output)
{

	//计算宽度和高度，并分配空间
	int width, height;
	width = std::floor((max_p_.x() - min_p_.x()) / window_size) + 1;
	height = std::floor((max_p_.y() - min_p_.y()) / window_size) + 1;
	output.resize(width * height);

	//将所有索引设成-1
	std::fill(begin(output), end(output), -1);


	//对所有点，判断其落入哪个格网，并且判断是否比格网内现有的点要低
	for (int i = 0; i < input.size(); ++i)
	{
		PointT pt = (*this)[input[i]];

		int row, col;
		row = std::floor((pt.y - min_p_.y()) / window_size);
		col = std::floor((pt.x - min_p_.x()) / window_size);

		int pos = row * width + col;
		int idx2 = output[pos];
		if (idx2 == -1)
		{
			output[pos] = input[i];
		}
		else
		{
			const auto& pt2 = (*this)[output[pos]];
			if (pt.z < pt2.z)
			{
				output[pos] = input[i];
			}
		}
	}

	//去除output 中index 是-1的点
	auto new_end = std::remove_if(begin(output), end(output), [](const int v){
		return (v == -1);
	});
	output.erase(new_end, end(output));
}

template <typename PointT>
void PyramidExclusive<PointT>::removeNaN(PointCloudPtr& output, float window_size)
{

}

template <typename PointT>
void PyramidExclusive<PointT>::checkNaN(const PointCloudPtr& cloud)
{

}

template <typename PointT>
void PyramidExclusive<PointT>::getVonNeumannNeighbour(const PointCloudPtr& input, int row, int col, PointT& out_pt)
{

}

}//end oil
